/*!
  @file image_publisher.cpp
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#include "mrover_vo/image_publisher.h"

#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/CameraInfo.h>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <fstream>  // NOLINT
#include <string>

using std::string;
using std::ifstream;

namespace vo {

void ImagePublisher::publishOneImage() {
  ROS_INFO_STREAM("opening " << filename_);
  cv::Mat image = cv::imread(filename_, 1);
  if (!image.data) {
    ROS_ERROR("failed opening image file");
    ROS_BREAK();
  }

  cv_bridge::CvImage::Ptr cv_ptr(new cv_bridge::CvImage);
  cv_ptr->header.stamp = ros::Time::now();
  cv_ptr->header.frame_id = "camera_frame";
  cv_ptr->encoding = "bgr8";
  cv_ptr->image = image;

  sensor_msgs::CameraInfo::Ptr info_ptr(new sensor_msgs::CameraInfo);
  info_ptr->header.stamp = cv_ptr->header.stamp;
  info_ptr->header.frame_id = cv_ptr->header.frame_id;
  info_ptr->width = cv_ptr->image.cols;
  info_ptr->height = cv_ptr->image.rows;

  img_pub_.publish(cv_ptr->toImageMsg(), info_ptr);
}

void ImagePublisher::publishStereoImages() {
  ifstream ifsL(imgL_list_.c_str(), ifstream::in);
  ifstream ifsR(imgR_list_.c_str(), ifstream::in);
  if (!ifsL.is_open() || !ifsR.is_open()) {
    ROS_ERROR("File open");
    ROS_BREAK();
  }

  cv_bridge::CvImage::Ptr cv_ptrL(new cv_bridge::CvImage);
  cv_bridge::CvImage::Ptr cv_ptrR(new cv_bridge::CvImage);
  cv_ptrL->header.frame_id = "camera_frameL";
  cv_ptrR->header.frame_id = "camera_frameR";
  cv_ptrL->header.stamp = cv_ptrR->header.stamp = ros::Time::now();
  cv_ptrL->encoding = cv_ptrR->encoding = "bgr8";

  sensor_msgs::CameraInfo::Ptr info_ptrL(new sensor_msgs::CameraInfo);
  sensor_msgs::CameraInfo::Ptr info_ptrR(new sensor_msgs::CameraInfo);
  info_ptrL->header.stamp = info_ptrR->header.stamp = cv_ptrL->header.stamp;
  info_ptrL->header.frame_id = cv_ptrL->header.frame_id;
  info_ptrR->header.frame_id = cv_ptrR->header.frame_id;
  info_ptrL->width  = info_ptrR->width  = cv_ptrL->image.cols;
  info_ptrL->height = info_ptrR->height = cv_ptrL->image.rows;

  string lineL, lineR;
  ros::Rate r(sim_rate_);
  while (getline(ifsL, lineL) && getline(ifsR, lineR)) {
    cv_ptrL->image = cv::imread(lineL, 1);
    cv_ptrR->image = cv::imread(lineR, 1);
    imgL_pub_.publish(cv_ptrL->toImageMsg(), info_ptrL);
    imgR_pub_.publish(cv_ptrR->toImageMsg(), info_ptrR);
    ros::spinOnce();
    r.sleep();
  }
}

}  // namespace vo

int main(int ac, char **av) {
  ros::init(ac, av, "ImagePublisher");
  ros::NodeHandle nh;
  vo::ImagePublisher handler(nh);
  ros::Duration(3.0).sleep();
  // handler.publishOneImage();
  handler.publishStereoImages();
  ros::spin();
  return 0;
}
